#include "stdafx.h"
#include "cvxAdaptiveQuadTree.h"
#include "nvmath.h"

using cvx::Bw32FImage;
using cvx::Rgb8UImage;
using cvx::Bw8UImage;

/************************************************************************/
/*
	CvxAdaptiveQuadTree
*/
/************************************************************************/

CvxAdaptiveQuadTree::CvxAdaptiveQuadTree(unsigned int level, const Mat &img, int threshold):CvxQuadTree(level)
{
	m_orgimg = img.clone();
	m_threshold = threshold;
	m_unknownNum = 0;
	for (int i = 0; i<level; ++i)
	{
		int sz = 1<<i;
		Mat mat  = cv::Mat(sz+1, sz+1, CV_32F);
		m_ShadingMatVec.push_back(mat);
		Mat aMat = cv::Mat(sz+1, sz+1, CV_32FC3);		
		m_AvectorMatVec.push_back(aMat);
	}
}
CvxAdaptiveQuadTree::CvxAdaptiveQuadTree(const CvxAdaptiveQuadTree &tree):CvxQuadTree(tree.m_N_LEVELS)
{
	m_orgimg = tree.m_orgimg.clone();
	m_threshold = tree.m_threshold;	
	m_unknownNum = 0;
	m_nodeNum = tree.m_nodeNum;

	for (int i = 0; i<m_N_LEVELS; ++i)
	{
		int sz = 1<<i;
		Mat mat  = cv::Mat(sz+1, sz+1, CV_32F);
		m_ShadingMatVec.push_back(mat);
		Mat aMat = cv::Mat(sz+1,sz+1, CV_32FC3);		
		m_AvectorMatVec.push_back(aMat);
	}

	m_root = new QtNode;
	m_root->_level = m_ROOT_LEVEL;	
	cloneTree(tree.m_root, m_root);
	
}

CvxAdaptiveQuadTree::~CvxAdaptiveQuadTree()
{

}

void CvxAdaptiveQuadTree::reverse(QtNode *node)
{
	if (node == NULL)
	{
		m_unknownNum = 0;
		reverse(m_root);
	}
	else if (node->isLeaf() && node->_data)
	{
		QtData *pData = (QtData*)node->_data;
		if (pData->_idx == -1)
		{
			pData->_idx = m_unknownNum++;
		}
		else
		{
			pData->_idx = -1;
		}
	}
	else
	{
		for (int i = 0; i<4; ++i)
		{
			if (node->_children[i] != NULL)
			{
				reverse(node->_children[i]);
			}			
		}
	}
}

void CvxAdaptiveQuadTree::updateUnknownIndex(const int baseIndex, QtNode *node)
{
	if (node == NULL)
	{
		updateUnknownIndex(baseIndex, m_root);
	}
	else if (node->isLeaf() && node->_data)
	{
		QtData *pData = (QtData*)node->_data;
		if (pData->_idx != -1)
		{
			pData->_idx += baseIndex;
		}	
	}
	else
	{
		for (int i = 0; i<4; ++i)
		{
			if (node->_children[i] != NULL)
			{
				updateUnknownIndex(baseIndex, node->_children[i]);
			}			
		}
	}
}

void CvxAdaptiveQuadTree::matchNode(float rgbThreshold, const int sizeThreshold, QtNode *node, CvxAdaptiveQuadTree *otherTree)
{
	assert(otherTree);

	if(node == NULL)
	{
		matchNode(rgbThreshold, sizeThreshold, m_root, otherTree);
	}
	else if(node->isLeaf() && node->_data)
	{
		QtData *data = (QtData *)node->_data;
		int sz = (1<<node->_level);
		int cx = node->_xLocCode + sz/2;
		int cy = node->_yLocCode + sz/2;
		int r,g,b;
		r = data->_r;
		g = data->_g;
		b = data->_b;

		QtNode *bestMatchNode = NULL;
		int dif = INT_MAX;

		// local the corresponding node in the right image
		QtNode *otherNode = otherTree->LocateNode(cx, cy);
		QtData *oData = (QtData *)(otherNode->_data);
		int curDif = data->difColor(oData, rgbThreshold);
		if(curDif < dif && (1<<otherNode->_level) >= sizeThreshold)
		{
			dif = curDif;
			bestMatchNode = otherNode;
		}

		//left node
		{			
			QtNode *n1 = NULL;
			QtNode *n2 = NULL;
			otherTree->leftNeighbors(otherNode, n1, n2);

			if(n1 && n1->size() >= sizeThreshold)
			{
				QtData *d = (QtData *)(n1->_data);
				int curDif = data->difColor(d, rgbThreshold);
				if(curDif < dif)
				{
					dif           = curDif;
					bestMatchNode = n1;
				}
			}
			if(n2 && n2->size() >= sizeThreshold)
			{
				QtData *d = (QtData *)(n2->_data);
				int curDif = data->difColor(d, rgbThreshold);
				if(curDif < dif)
				{
					dif           = curDif;
					bestMatchNode = n2;
				}
			}
		}

		//right node
		{
			QtNode *n1 = NULL;
			QtNode *n2 = NULL;
			otherTree->rightNeighbors(otherNode, n1, n2);

			if(n1 && n1->size() >= sizeThreshold)
			{
				QtData *d = (QtData *)(n1->_data);
				int curDif = data->difColor(d, rgbThreshold);
				if(curDif < dif)
				{
					dif           = curDif;
					bestMatchNode = n1;
				}
			}
			if(n2 && n2->size() >= sizeThreshold)
			{
				QtData *d = (QtData *)(n2->_data);
				int curDif = data->difColor(d, rgbThreshold);
				if(curDif < dif)
				{
					dif           = curDif;
					bestMatchNode = n2;
				}
			}
		}

		//top node
		if(1)
		{
			QtNode *n1 = NULL;
			QtNode *n2 = NULL;
			otherTree->topNeighbors(otherNode, n1, n2);

			if(n1 && n1->size() >= sizeThreshold)
			{
				QtData *d = (QtData *)(n1->_data);
				int curDif = data->difColor(d, rgbThreshold);
				if(curDif < dif)
				{
					dif           = curDif;
					bestMatchNode = n1;
				}
			}
			if(n2 && n2->size() >= sizeThreshold)
			{
				QtData *d = (QtData *)(n2->_data);
				int curDif = data->difColor(d, rgbThreshold);
				if(curDif < dif)
				{
					dif           = curDif;
					bestMatchNode = n2;
				}
			}

		}

		//bottom node
		if(1)
		{
			QtNode *n1 = NULL;
			QtNode *n2 = NULL;
			otherTree->bottomNeighbors(otherNode, n1, n2);

			if(n1 && n1->size() >= sizeThreshold)
			{
				QtData *d = (QtData *)(n1->_data);
				int curDif = data->difColor(d, rgbThreshold);
				if(curDif < dif)
				{
					dif           = curDif;
					bestMatchNode = n1;
				}
			}
			if(n2 && n2->size() >= sizeThreshold)
			{
				QtData *d = (QtData *)(n2->_data);
				int curDif = data->difColor(d, rgbThreshold);
				if(curDif < dif)
				{
					dif           = curDif;
					bestMatchNode = n2;
				}
			}

		}

		//record best math node
		data->_bestMatchNode = bestMatchNode;
	}
	else
	{		
		int sz = (1<<node->_level);
		if(sz >= sizeThreshold)
		{
			for(int i = 0; i<4; i++)
			{
				if(node->_children[i] != NULL)
				{
					matchNode(rgbThreshold, sizeThreshold, node->_children[i], otherTree);
				}
			}
		}		
	}
}

void CvxAdaptiveQuadTree::crossMatchNode(QtNode *node, CvxAdaptiveQuadTree *otherTree)
{
	assert(otherTree);
	
	if(node == NULL)
	{		
		crossMatchNode(m_root, otherTree);
	}
	else if(node->isLeaf())
	{
		QtData *d = (QtData *) (node->_data);
		assert(d);
		QtNode *n = d->_bestMatchNode;		
		if(n)
		{
			QtNode *n2 = ((QtData *) (n->_data))->_bestMatchNode;
			if(n2 != node)
			{
				d->_bestMatchNode = NULL;				
			}			
		}
	}
	else
	{
		for(int i = 0; i<4; i++)
		{
			if((node->_children[i]) != NULL)
			{
				crossMatchNode(node->_children[i], otherTree);
			}
		}
	}
}


void CvxAdaptiveQuadTree::create(QtNode * node)
{
	int h = m_orgimg.rows;
	int w = m_orgimg.cols;
	//root node
	if (node == NULL)
	{
		m_root = new QtNode;
		m_root->createChildrenNode();
		m_root->_level = m_ROOT_LEVEL;
		m_root->_children[0]->set(false, false, m_root);
		m_root->_children[1]->set(true, false, m_root);
		m_root->_children[2]->set(false, true, m_root);
		m_root->_children[3]->set(true, true, m_root);

		//delete unnecessary node
		node = m_root;

		//second child
		if (node->_children[1]->_xLocCode >= w)
		{
			delete node->_children[1];
			node->_children[1] = NULL;				
		}			
		//third child
		if (node->_children[2]->_yLocCode >= h)
		{
			delete node->_children[2];
			node->_children[2] = NULL;
		}		

		//forth child
		if (node->_children[3]->_yLocCode >= h || node->_children[3]->_xLocCode >= w)
		{
			delete node->_children[3];
			node->_children[3] = NULL;
		}

		for (int i = 0; i<4; ++i)
		{
			if (node->_children[i] != NULL)
			{
				create(node->_children[i]);
			}			
		}
	}
	else if (node->_level > 0)
	{
		int sz = 1<<node->_level;
		cv::Rect r1 = cv::Rect(node->_xLocCode, node->_yLocCode, sz, sz);
		if (r1.x + sz <= m_orgimg.cols && r1.y + sz <= m_orgimg.rows)  //whole area is in image scope
		{
			Mat m1 = m_orgimg(r1);
			Scalar mean, stdDev;		
			cv::meanStdDev(m1, mean, stdDev);
			if (stdDev[0] > m_threshold || stdDev[1] > m_threshold || stdDev[2] > m_threshold)
			{
				node->createChildrenNode();
				node->_children[0]->set(false, false, node);
				node->_children[1]->set(true, false, node);
				node->_children[2]->set(false, true, node);
				node->_children[3]->set(true, true, node);
				for (int i = 0; i<4; ++i)
				{
					create((node->_children[i]));
				}			
			}		
			
		}
		else
		{
			node->createChildrenNode();
			node->_children[0]->set(false, false, node);
			node->_children[1]->set(true, false, node);
			node->_children[2]->set(false, true, node);
			node->_children[3]->set(true, true, node);

			//first child must be in the scope			

			//second child
			if (node->_children[1]->_xLocCode >= w)
			{
				delete node->_children[1];
				node->_children[1] = NULL;				
			}			
			//third child
			if (node->_children[2]->_yLocCode >= h)
			{
				delete node->_children[2];
				node->_children[2] = NULL;
			}		

			//forth child
			if (node->_children[3]->_yLocCode >= h || node->_children[3]->_xLocCode >= w)
			{
				delete node->_children[3];
				node->_children[3] = NULL;
			}

			//only create scope that in image
			for (int i = 0; i<4; ++i)
			{
				if (node->_children[i] != NULL)
				{
					create(node->_children[i]);
				}				
			}
		}		
	}	
}

void CvxAdaptiveQuadTree::balance()
{
	bool changed = true;
	while(changed)
	{
		changed = false;
		this->ConstructRestrictedQuadTree(NULL, changed);
	}
}

void CvxAdaptiveQuadTree::ConstructRestrictedQuadTree(QtNode *node, bool& changed)
{
	//start from root
	if (node == NULL)
	{
		node = m_root;
	}
	//leaf node
	if (node->isLeaf())
	{
		unsigned int level = node->_level;
		QtNode *pLeft = LocateLeftNeighbor(node);
		if (pLeft && pLeft->_level > level + 1){
			SubdivideNode(pLeft);
			changed = true;				
		}

		QtNode *pRight = LocateRightNeighbor(node);
		if (pRight && pRight->_level > level + 1){
			SubdivideNode(pRight);
			changed = true;
		}
		QtNode *pBottom = LocateBottomNeighbor(node);
		if (pBottom && pBottom->_level > level + 1){
			SubdivideNode(pBottom);
			changed = true;			
		}

		QtNode *pTop = LocateTopNeighbor(node);		
		if(pTop && pTop->_level > level + 1){
			SubdivideNode(pTop);
			changed = true;
		}		
	}
	else
	{
		for (int i = 0; i<4; ++i)
		{
			if (node->_children[i] != NULL)
			{
				ConstructRestrictedQuadTree(node->_children[i], changed);
			}			
		}
	}

}



void CvxAdaptiveQuadTree::initLeafData(const Mat &img, const Mat &shadingMap, const Mat &knownMap, 
										 QtNode *node, const Mat&edgeMap)
{
	assert(shadingMap.depth() == CV_32F);
	assert(knownMap.depth() == CV_8U);
	assert(edgeMap.depth() == CV_8U);

	if (node == NULL)
	{
		node = m_root;
		m_unknownNum = 0;
		m_nodeNum = 0;
		initLeafData(img, shadingMap, knownMap, node, edgeMap);
	}
	else if (node->isLeaf())  //leaf node
	{
		int sz = 1<<node->_level;
		int x = node->_xLocCode;
		int y = node->_yLocCode;
		if (sz != 1)
		{			
			cv::Rect r1 = cv::Rect(x, y, sz, sz);
			Mat m1 = img(r1);
			Scalar mean, stdDev;		
			cv::meanStdDev(m1, mean, stdDev);
			QtData *pData = new QtData;
 			pData->_b = mean[0];
 			pData->_g = mean[1];
 			pData->_r = mean[2];
			pData->_edge = (edgeMap.at<unsigned char>(y, x) == 255 || 
							edgeMap.at<unsigned char>(y+sz-1, x+sz-1) == 255);
			pData->_l = 0.3 * pData->_r + 0.59 * pData->_g + 0.11 * pData->_b;
			pData->_minSha = std::max(mean[0], mean[1]);
			pData->_minSha = std::max(mean[2], pData->_minSha);
			pData->_minSha = pData->_minSha/255.0;
			pData->_minSha = std::min(0.95, pData->_minSha*1.2);

			if (knownMap.at<unsigned char>(y, x) == 255 &&
				knownMap.at<unsigned char>(y+sz-1, x+sz-1) == 255 &&
				knownMap.at<unsigned char>(y+sz-1, x) == 255 &&
				knownMap.at<unsigned char>(y, x+sz-1) == 255)
			{
				//double check every pixel is known
				bool isKnown = true;
				Mat sKnownMap = knownMap(r1);
				for (int sy = 0; sy<r1.height; ++sy)
				{
					for (int sx = 0; sx<r1.width; ++sx)
					{
						if (sKnownMap.at<unsigned char>(sy, sx) != 255)
						{
							isKnown = false;
							break;
						}
					}
					if (!isKnown)
					{
						break;
					}
				}
				if (isKnown)
				{
					Mat s1 = shadingMap(r1);
					cv::meanStdDev(s1, mean, stdDev);
					pData->_s = mean[0]; //set the mean value as node shading value, means a node has one shading value
					pData->_idx = -1;
				}
				else
				{
					//some pixel is unknown
					pData->_idx = m_unknownNum++;
					pData->_s = 0.0;
				}				
			}
			else
			{
				pData->_idx = m_unknownNum++;
				pData->_s = 0.0;
			}
			pData->_nodeIdx = m_nodeNum++;
			node->_data = (void *)pData;
		}
		else //sz == 1, means a single pixel
		{
			const unsigned char *p = img.ptr(y, x);
			QtData *pData = new QtData;
			pData->_b = p[0];
			pData->_g = p[1];
			pData->_r = p[2];
			pData->_edge = (edgeMap.at<unsigned char>(y, x) == 255);
			pData->_l = 0.3 * pData->_r + 0.59 * pData->_g + 0.11 * pData->_b;
			pData->_minSha = std::max(p[0], p[1]);
			pData->_minSha = std::max(double(p[2]), pData->_minSha);
			pData->_minSha = pData->_minSha/255.0;
			pData->_minSha = std::min(0.95, pData->_minSha*1.2);		
			if (knownMap.at<unsigned char>(y, x) == 255)
			{
				pData->_s   = shadingMap.at<float>(y, x);
				pData->_idx = -1;
			}
			else
			{
				pData->_idx = m_unknownNum++;
				pData->_s = 0.0;
			}
			pData->_nodeIdx = m_nodeNum++;
			node->_data = (void *)pData;
		}
	}
	else
	{
		for (int i = 0; i<4; ++i)
		{
			if (node->_children[i])
			{
				initLeafData(img, shadingMap, knownMap, node->_children[i], edgeMap);
			}			
		}
	}
}

void CvxAdaptiveQuadTree::adaptiveInitLeafData(const Mat &img, const Mat &edgeMap, QtNode *node, int threshold, float minimumShadingScale)
{
	if(node == NULL)
	{
		node = m_root;
		adaptiveInitLeafData(img, edgeMap, node, threshold, minimumShadingScale);
	}
	else if(node->isLeaf())
	{
		int sz = 1<<node->_level;
		int x = node->_xLocCode;
		int y = node->_yLocCode;
		const unsigned char *edgeValue = edgeMap.ptr(y, x);
		if (sz != 1 || edgeValue[0] != 255)
		{			
			cv::Rect r1 = cv::Rect(x, y, sz, sz);
			Mat m1 = img(r1);
			Scalar mean, stdDev;
			cv::meanStdDev(m1, mean, stdDev);
			QtData *pData = new QtData;
 			pData->_b = mean[0];
 			pData->_g = mean[1];
 			pData->_r = mean[2];
			pData->_l = (1.0 * pData->_r + pData->_g + pData->_b)/3.0;
			pData->_minSha = std::max(mean[0], mean[1]);
			pData->_minSha = std::max(mean[2], pData->_minSha);
			pData->_minSha = pData->_minSha/255.0;
			pData->_minSha = std::min(0.95, pData->_minSha * minimumShadingScale);

			// assume shading value in this area is knownn, if the node size large than a threshold 
			if (sz >= threshold)
			{
				pData->_s   = pData->_minSha; 
				pData->_idx = -1;				
			}
			else
			{
				pData->_s = 0.0;
				pData->_idx = m_unknownNum++;				
			}
			pData->_nodeIdx = m_nodeNum++;
			node->_data = (void *)pData;
		}
		else //sz == 1, means a single pixel
		{
			const unsigned char *p = img.ptr(y, x);
			QtData *pData = new QtData;
			pData->_b = p[0];
			pData->_g = p[1];
			pData->_r = p[2];
			pData->_l = (1.0 * pData->_r + pData->_g + pData->_b)/3.0;
			pData->_minSha = std::max(p[0], p[1]);
			pData->_minSha = std::max(double(p[2]), pData->_minSha);
			pData->_minSha = pData->_minSha/255.0;
			pData->_minSha = std::min(0.95, pData->_minSha * minimumShadingScale);

			//sz == 1, means a konwn shading pixel
			pData->_s   = pData->_minSha;
			pData->_idx = -1;
			
			pData->_nodeIdx = m_nodeNum++;
			node->_data = (void *)pData;
		}
	}
	else
	{
		for(int i = 0; i<4; i++)
		{
			if(node->_children[i])
			{
				adaptiveInitLeafData(img, edgeMap, node->_children[i], threshold, minimumShadingScale);
			}
		}
	}
}

void CvxAdaptiveQuadTree::clusterLargeNode(QtNode *node, const int sizeThreshold, const Mat &labelMap, const int labelNum)
{
	if(node == NULL)
	{
		assert(labelMap.elemSize() == 1);
		clusterLargeNode(m_root, sizeThreshold, labelMap, labelNum);
	}
	else if (node->isLeaf())
	{
		int sz = 1<<node->_level;
		if(sz >= sizeThreshold)
		{
			QtData *pdata = (QtData *) (node->_data);
			assert(pdata);

			//check for four corners
			const int x = node->_xLocCode;
			const int y = node->_yLocCode;
			int refIdx  = labelMap.at<unsigned char>(y, x);

			int cx = x + sz;
			int cy = y;
			int cRefIdx = labelMap.at<unsigned char>(cy, cx);

			//different color set
			if(cRefIdx != refIdx) return;

			cx = x + sz;
			cy = y + sz;
			cRefIdx = labelMap.at<unsigned char>(cy, cx);
			if(cRefIdx != refIdx) return;

			cx = x;
			cy = y + sz;
			cRefIdx = labelMap.at<unsigned char>(cy, cx);
			if(cRefIdx != refIdx) return;

			pdata->_similarRefIdx = refIdx;
		}
	}
	else
	{
		int sz = 1<<node->_level;
		if(sz >= sizeThreshold)
		{
			for(int i = 0; i<4; i++)
			{
				if(node->_children[i])
				{
					clusterLargeNode(node->_children[i], sizeThreshold, labelMap, labelNum);
				}
			}
		}
	}


}

void CvxAdaptiveQuadTree::updateEdgeNodeShading(QtNode *node, Mat &shadingMap, Mat &edgeMap, Mat &albedo, int winSize)
{
	if(node ==  NULL)
	{
		updateEdgeNodeShading(m_root, shadingMap, edgeMap, albedo, winSize);
	}
	else if(node->isLeaf())
	{
		QtData *data = (QtData*)(node->_data);
		if(data->_edge)
		{
			int r = data->_r;
			int g = data->_g;
			int b = data->_b;

			//nominant color
			int k = 0;
			if(g > r)
			{
				k = 1;
				if(b > g)
				{
					k = 2;
				}
			}
			if(b > r)
			{
				k = 2;
				if(g > b)
				{
					k = 1;
				}
			}

			int dif_min = INT_MAX;
			int x = node->_xLocCode;
			int y = node->_yLocCode;

			Rgb8UImage org_img(&m_orgimg);
			Bw32FImage sha_img(&shadingMap);
			Bw8UImage edge_map(&edgeMap);
			Rgb8UImage albedo_map(&albedo);

			int h = m_orgimg.rows;
			int w = m_orgimg.cols;
			for(int i = -2; i<=2; i++)
			{
				for(int j = -2; j<=2; j++)
				{
					if(abs(i) > 1 || abs(j) > 1)
					{
						int cy = y + i;
						int cx = x + j;

						//assume it it inside image ? and not an edge
						if(cy >= 0 && cy < h && cx >= 0 && cx < w && edge_map[cy][cx] == 0)
						{
							int curDif = abs(org_img[y][x].r - r) + abs(org_img[y][x].g - g) + abs(org_img[y][x].b - b);
							if(curDif < dif_min)
							{
								dif_min = curDif;
								if(k == 0)
								{
									data->_minSha = 1.0 * r/albedo_map[cy][x].r;
								}
								else if(k == 1)
								{
									data->_minSha = 1.0 * g/albedo_map[cy][x].g;
								}
								else
								{
									data->_minSha = 1.0 * b/albedo_map[cy][x].b;
								}						
								
								data->_s = data->_minSha;
							}
						}
					}
				}
			}
		}

	}
	else
	{
		for(int i = 0; i<4; i++)
		{
			if(node->_children[i])
			{
				updateEdgeNodeShading(node->_children[i], shadingMap, edgeMap, albedo, winSize);
			}
		}

	}
}

int CvxAdaptiveQuadTree::leftNeighbors(QtNode *cell, QtNode *&n1, QtNode *&n2)
{
	QtNode *pNode = LocateLeftNeighbor(cell);
	if (pNode)
	{
		if (pNode->isLeaf())
		{
			n1 = pNode;
			n2 = NULL;
			return 1;
		}
		else 
		{
			n1 = (pNode->_children[1]);
			n2 = (pNode->_children[3]);
			return 2;
		}
	}
	else
	{
		n1 = NULL;
		n2 = NULL;
		return 0;
	}
}

int CvxAdaptiveQuadTree::bottomNeighbors(QtNode *cell, QtNode *&n1, QtNode *&n2)
{
	QtNode *pNode = LocateBottomNeighbor(cell);
	if (pNode)
	{
		if (pNode->isLeaf())
		{
			n1 = pNode;
			n2 = NULL;
			return 1;
		}
		else 
		{
			n1 = (pNode->_children[2]);
			n2 = (pNode->_children[3]);
			return 2;
		}
	}
	else
	{
		n1 = NULL;
		n2 = NULL;
		return 0;
	}
}

int CvxAdaptiveQuadTree::rightNeighbors(QtNode *cell, QtNode *&n1, QtNode *&n2)
{
	QtNode *pNode = LocateRightNeighbor(cell);
	if (pNode)
	{
		if (pNode->isLeaf())
		{
			n1 = pNode;
			n2 = NULL;
			return 1;
		}
		else 
		{
			n1 = (pNode->_children[0]);
			n2 = (pNode->_children[2]);
			return 2;
		}
	}
	else
	{
		n1 = NULL;
		n2 = NULL;
		return 0;
	}
}

int CvxAdaptiveQuadTree::topNeighbors(QtNode *cell, QtNode *&n1, QtNode *&n2)
{
	QtNode *pNode = LocateTopNeighbor(cell);
	if (pNode)
	{
		if (pNode->isLeaf())
		{
			n1 = pNode;
			n2 = NULL;
			return 1;
		}
		else 
		{
			n1 = (pNode->_children[0]);
			n2 = (pNode->_children[1]);
			return 2;
		}
	}
	else
	{
		n1 = NULL;
		n2 = NULL;
		return 0;
	}
}

QtNode *CvxAdaptiveQuadTree::LocateNode (int x, int y)
{
	if ( x>=m_MAX_VAL || y >= m_MAX_VAL) return NULL;
	if ( x>=m_orgimg.cols || y >= m_orgimg.rows) return NULL;

	unsigned int level = m_ROOT_LEVEL - 1;
	QtNode *pNode = m_root;
	TraverseToPosition(pNode, level, x, y);
	return pNode;
}


QtNode *CvxAdaptiveQuadTree::LocateRightNeighbor (QtNode *cell)
{
	//----No right neighbor if this is the right side of the quadtree
	unsigned int binaryCellSize = 1 << cell->_level;
	if ((cell->_xLocCode + binaryCellSize) >= (m_orgimg.cols)) return NULL;
	else {
		//----Get cell's x and y locational codes and the x locational code of the
		//----cell's right neighbors
		unsigned int xLocCode = cell->_xLocCode;
		unsigned int yLocCode = cell->_yLocCode;
		unsigned int xRightLocCode = xLocCode + binaryCellSize;


		//----Determine the smallest common ancestor of the cell and the cell's 
		//----right neighbors 
		unsigned int cellLevel, nextLevel;
		unsigned int diff = xLocCode ^ xRightLocCode;
		QtNode *pCell = cell;
		cellLevel = nextLevel = cell->_level;
		GetCommonAncestor(pCell, nextLevel, diff);	

		//----Start from the smallest common ancestor and follow the branching 
		//----patterns of the locational codes downward to the smallest right
		//----neighbor of size greater than or equal to cell
		nextLevel--;
		TraverseToLevel(pCell, nextLevel, xRightLocCode, yLocCode, cellLevel);		                                                             
		return(pCell);
	}

}
QtNode *CvxAdaptiveQuadTree::LocateTopNeighbor(QtNode *cell)
{
	//----No Top neighbor if this is the top side of the quadtree
	unsigned int binaryCellSize = 1 << cell->_level;
	if ((cell->_yLocCode + binaryCellSize) >= (m_orgimg.rows)) return NULL;
	else {
		//----Get cell's x and y locational codes and the x locational code of the
		//----cell's right neighbors
		unsigned int xLocCode = cell->_xLocCode;
		unsigned int yLocCode = cell->_yLocCode;	
		unsigned int yTopLocCode = yLocCode + binaryCellSize;

		//----Determine the smallest common ancestor of the cell and the cell's 
		//----top neighbors 
		unsigned int cellLevel, nextLevel;
		unsigned int diff = yLocCode ^ yTopLocCode;

		QtNode *pCell = cell;
		cellLevel = nextLevel = cell->_level;		
		GetCommonAncestor(pCell, nextLevel, diff);	

		//----Start from the smallest common ancestor and follow the branching 
		//----patterns of the locational codes downward to the smallest top
		//----neighbor of size greater than or equal to cell
		nextLevel--;	
		TraverseToLevel(pCell, nextLevel, xLocCode, yTopLocCode, cellLevel);
		return(pCell);
	}
}

void CvxAdaptiveQuadTree::draw(QtNode * node)
{
	if (node == NULL)
	{
		m_orgBackUp = m_orgimg.clone();
		draw(m_root);		
	}
	else if (node->isLeaf())
	{
		int sz = 1<<node->_level;		
		cv::rectangle(m_orgBackUp, Rect(node->_xLocCode, node->_yLocCode, sz, sz), Scalar(0,0,255,0));
	}
	else
	{
		for (int i = 0; i<4; ++i)
		{
			if ((node->_children[i]) != NULL)
			{
				draw((node->_children[i]));
			}			
		}
	}
}

void CvxAdaptiveQuadTree::drawColorNode(QtNode *node, const Scalar& colorA, const Scalar& colorB)
{
	if (node == NULL)
	{
		m_orgBackUp = m_orgimg.clone();
		drawColorNode(m_root, colorA, colorB);		
	}
	else if (node->isLeaf())
	{
		int sz = 1<<node->_level;

		QtData *pData = (QtData *)(node->_data);
		if(pData->_idx == -1)
		{
			cv::rectangle(m_orgBackUp, Rect(node->_xLocCode, node->_yLocCode, sz, sz), colorA);
		}
		else 
		{
			cv::rectangle(m_orgBackUp, Rect(node->_xLocCode, node->_yLocCode, sz, sz), colorB);
		}
	}
	else
	{
		for (int i = 0; i<4; ++i)
		{
			if ((node->_children[i]) != NULL)
			{
				drawColorNode((node->_children[i]), colorA, colorB);
			}			
		}
	}
}

void CvxAdaptiveQuadTree::drawClusteredNode(QtNode *node, int labelNum, vector<Scalar> &colors)
{
	if(node == NULL)
	{
		assert(labelNum >= 1);

		colors.resize(labelNum);
		for(int i = 0; i<labelNum; i++)
		{
			colors[i][0] = (rand()%256)/2.0;
			colors[i][1] = (rand()%256)/2.0;
			colors[i][2] = (rand()%256)/2.0;
		}
		
		m_orgBackUp = m_orgimg.clone();
		drawClusteredNode(m_root, labelNum, colors);
	}
	else if(node->isLeaf())
	{
		QtData *pdata = (QtData*) node->_data;
		assert(pdata);
		int sz = 1<<node->_level;
		if(pdata->_similarRefIdx != -1)
		{
			Mat subImage   = m_orgBackUp(cv::Rect(node->_xLocCode, node->_yLocCode, sz, sz));
			Mat colorImage = Mat(subImage.rows, subImage.cols, CV_8UC3, colors[pdata->_similarRefIdx]);
			
			//blender original color and mark colors
			cv::scaleAdd(subImage, 0.5, colorImage, subImage);
		}

	}
	else
	{
		for(int i = 0; i<4; i++)
		{
			if(node->_children[i])
			{
				drawClusteredNode(node->_children[i], labelNum, colors);
			}
		}

	}

}

void CvxAdaptiveQuadTree::setShadingResult(Mat &shaImg, QtNode *node, const vector<double> &rst)
{
	assert(shaImg.depth() == CV_32F);

	if (node == NULL)
	{
		setShadingResult(shaImg, m_root, rst);
	}
	else if (node->isLeaf())
	{
		if (node->_data != NULL)
		{
			int sz = 1<<node->_level;
			QtData *pData = (QtData *)(node->_data);
			assert(pData);
			if (pData->_idx != -1)
			{					
				double s = rst[pData->_idx];
				if (s < pData->_minSha)
				{
					s = pData->_minSha;
				}
				pData->_s = s;
				if (sz != 1)
				{
					cv::Rect r1 = Rect(node->_xLocCode, node->_yLocCode, sz, sz);
					Mat m1 = shaImg(r1);
					m1 = Scalar(s, s, s, 1.0);
				}
				else
				{
					shaImg.at<float>(node->_yLocCode, node->_xLocCode) = s;					
				}
			}
			else
			{
				double s = pData->_s;
				if (sz != 1)
				{
					cv::Rect r1 = Rect(node->_xLocCode, node->_yLocCode, sz, sz);
					Mat m1 = shaImg(r1);
					m1 = Scalar(s, s, s, 1.0);
				}
				else
				{
					shaImg.at<float>(node->_yLocCode, node->_xLocCode) = s;					
				}
			}
		}
	}
	else
	{
		for (int i =0 ; i<4; ++i)
		{
			if (node->_children[i] != NULL)
			{
				setShadingResult(shaImg, (node->_children[i]), rst);
			}			
		}
	}
}

void CvxAdaptiveQuadTree::upsampleShadingResult(Mat &shaImg, QtNode *node)
{
	assert(shaImg.depth() == CV_32F);

	if (node == NULL)
	{
		upsampleShadingResult(shaImg, m_root);
	}
	else if (node->isLeaf())
	{
		if (node->_data != NULL)
		{
			int sz = 1<<node->_level;
			QtData *pData = (QtData *)(node->_data);			
			double s = pData->_s;

			if (sz != 1)
			{
				Scalar albedoCol;
				albedoCol[0] = pData->_r/s;
				albedoCol[1] = pData->_g/s;
				albedoCol[2] = pData->_b/s;
				cv::Rect r1 = Rect(node->_xLocCode, node->_yLocCode, sz, sz);
				Mat smSha = shaImg(r1);
				Mat smOrg = m_orgimg(r1);
				
				Rgb8UImage sm_org(&smOrg);
				Bw32FImage sm_sha(&smSha);
				for (int y = 0; y<r1.height; ++y)
				{
					for (int x = 0; x<r1.width; ++x)
					{
						float lumi = sm_org[y][x].r * 0.3 + sm_org[y][x].g * 0.59 + sm_org[y][x].b * 0.11;												
						sm_sha[y][x] = s * lumi/pData->_l;
					}
				}
			}
			else
			{
				shaImg.at<float>(node->_yLocCode, node->_xLocCode) = s;					
			}
		}
	}
	else
	{
		for (int i =0 ; i<4; ++i)
		{
			if (node->_children[i] != NULL)
			{
				upsampleShadingResult(shaImg, (node->_children[i]));
			}			
		}
	}

}

void CvxAdaptiveQuadTree::drawKnownShading(QtNode *node, Mat &shadingMap)
{
	assert(shadingMap.depth() == CV_32F);
	if (node == NULL)
	{
		drawKnownShading(m_root, shadingMap);		
	}
	else if (node->isLeaf())
	{
		QtData *pData = (QtData*)node->_data;
		if (pData && pData->_s != 0.0)
		{
			int sz = 1<<node->_level;
			cv::Rect r1(node->_xLocCode, node->_yLocCode, sz, sz);
			shadingMap(r1) = Scalar(pData->_s, pData->_s, pData->_s, pData->_s);
		}
	}
	else
	{
		for (int i = 0; i<4; ++i)
		{
			if ((node->_children[i]) != NULL)
			{
				drawKnownShading(node->_children[i], shadingMap);
			}			
		}
	}
}

void CvxAdaptiveQuadTree::drawMatchNode(QtNode *node, CvxAdaptiveQuadTree *otherTree, Mat &resultImage)
{
	assert(otherTree);
	int gapWidth = 10;
	if(node == NULL)
	{
		// combine the left and right image into one image
		int h = m_orgimg.rows;
		int w = m_orgimg.cols;
		resultImage = Mat::zeros(h, w*2+gapWidth, CV_8UC3);
		m_orgimg.copyTo(resultImage(cv::Rect(0, 0, w, h)));
		otherTree->m_orgimg.copyTo(resultImage(cv::Rect(w+gapWidth, 0, w, h)));

		drawMatchNode(m_root, otherTree, resultImage);

	}
	else if(node->isLeaf())
	{
		QtData *d = (QtData *) (node->_data);
		assert(d);
		QtNode *n = d->_bestMatchNode;		
		if(n)
		{
			QtNode *n2 = ((QtData *) (n->_data))->_bestMatchNode;
	//		if(n2 == node)
			{
				int r = rand()%256;
				int g = rand()%256;
				int b = rand()%256;
				int sz = node->size();
				int nsz = n->size();
				int xLeftC = node->_xLocCode + sz/2;
				int yLeftC = node->_yLocCode + sz/2;
				int xRightC = n->_xLocCode + nsz/2;
				int yRightC = n->_yLocCode + nsz/2;

				cv::rectangle(resultImage, Rect(node->_xLocCode, node->_yLocCode, sz, sz), Scalar(b, g, r));
				cv::rectangle(resultImage, Rect(n->_xLocCode + m_orgimg.cols + gapWidth, n->_yLocCode, nsz, nsz), Scalar(b, g, r));
			}
			
		//	cv::line(resultImage, cv::Point(xLeftC, yLeftC), cv::Point(xRightC + m_orgimg.cols + gapWidth, yRightC), Scalar(b, g, r), 1);
		}
	}
	else
	{
		for(int i = 0; i<4; i++)
		{
			if((node->_children[i]) != NULL)
			{
				drawMatchNode(node->_children[i], otherTree, resultImage);
			}
		}
	}

}

void CvxAdaptiveQuadTree::compareGroundTruthWithGrayLevelInPixelLeaf(QtNode *node, Mat &shadingGroundTruth, vector<unsigned int> &positionIdx)
{
	if( node == NULL)
	{
		node = m_root;
		compareGroundTruthWithGrayLevelInPixelLeaf(node, shadingGroundTruth, positionIdx);
	}
	else if(node->isLeaf())
	{
		int sz = 1<<node->_level;
		if(sz == 1)  // pixel leaf
		{
			int y = node->_yLocCode;
			int x = node->_xLocCode;
			int w = shadingGroundTruth.cols;
			positionIdx.push_back(y * w + x);
		}
	}
	else
	{
		for(int i = 0; i<4; i++)
		{
			if((node->_children[i]) != NULL)
			{
				compareGroundTruthWithGrayLevelInPixelLeaf((node->_children[i]), shadingGroundTruth, positionIdx);
			}
		}
	}
}

void CvxAdaptiveQuadTree::setShadingToEdgeNode(QtNode *node, Mat &edgeMap, const Mat &shadingMap)
{
	assert(m_root);

	if(node == NULL)
	{
		setShadingToEdgeNode(m_root, edgeMap, shadingMap);
	}
	else if(node->isLeaf())
	{
		int x = node->_xLocCode;
		int y = node->_yLocCode;
		int sz = (1<<node->_level);
		if(sz == 1)
		{
		//	printf("sz == 1 \n");
			unsigned char *edge = edgeMap.ptr(y, x);
			if(edge[0] == 255)
			{
				QtData *pData = (QtData *)node->_data;
				pData->_minSha = (shadingMap.ptr(y, x))[0]/255.0;
				pData->_s = pData->_minSha;
		//		printf("set shading\n");
			}
		}		
	}
	else
	{
		for(int i = 0; i<4; i++)
		{
			if(node->_children[i])
			{
				setShadingToEdgeNode(node->_children[i], edgeMap, shadingMap);
			}			
		}
	}

}

void CvxAdaptiveQuadTree::InterpolateAvector(QtNode *node, Mat &AvectorMap)
{
	assert(AvectorMap.depth() == CV_32F);
	assert(AvectorMap.channels() == 3);
	if (node == NULL)
	{
		InterpolateAvector(m_root, AvectorMap);
	}
	else if (node->isLeaf() && node->_data)
	{
		int sz = 1<<node->_level;
		int x = node->_xLocCode, y = node->_yLocCode;
		QtData *pData = (QtData *)(node->_data);
		if (sz == 1)
		{
			//b g r
			float *p = (float *)AvectorMap.ptr(y, x);
			p[0] = pData->_avector[2];
			p[1] = pData->_avector[1];
			p[2] = pData->_avector[0];			
		}
		else
		{
			//get four corner vector value
 			float a1[3] = {pData->_avector[2], pData->_avector[1], pData->_avector[0]};
			float a2[3] = {pData->_avector[2], pData->_avector[1], pData->_avector[0]};
			float a3[3] = {pData->_avector[2], pData->_avector[1], pData->_avector[0]};
			float a4[3] = {pData->_avector[2], pData->_avector[1], pData->_avector[0]};

			QtNode *n1;
			n1 = LocateNode(x+sz, y);
			if (n1 != NULL)
			{
				a2[0] = ((QtData *)n1->_data)->_avector[2];
				a2[1] = ((QtData *)n1->_data)->_avector[1];
				a2[2] = ((QtData *)n1->_data)->_avector[0];
			}
 			n1 = LocateNode(x, y+sz);
 			if (n1 != NULL)
 			{
				a3[0] = ((QtData *)n1->_data)->_avector[2];
				a3[1] = ((QtData *)n1->_data)->_avector[1];
				a3[2] = ((QtData *)n1->_data)->_avector[0];
 			}
 			n1 = LocateNode(x+sz, y+sz);
 			if (n1 != NULL)
 			{
				a4[0] = ((QtData *)n1->_data)->_avector[2];
				a4[1] = ((QtData *)n1->_data)->_avector[1];
				a4[2] = ((QtData *)n1->_data)->_avector[0];
 			}

			//interpolate
			int zz1 = sz+1;
		//	Mat data = Mat(zz1, zz1, CV_32FC3);	
		//	cout<<m_AvectorMatVec.size()<<endl;
			Mat data = m_AvectorMatVec[node->_level];
			for (int j = 0; j<zz1; ++j)
			{
				float dis1 = j  - 0;
				float dis2 = zz1 - j;

				//first row
				float *p = (float *)data.ptr(0, j);
				for (int k = 0 ; k<3; ++k)
				{
					p[k] = a1[k] *dis2/zz1 + a2[k] * dis1/zz1;
				}

				//last row
				p = (float *)data.ptr(sz, j);
				for (int k = 0; k<3; ++k)
				{
					p[k] = a3[k] * dis2/zz1 + a4[k] * dis1/zz1;
				}				
			}
			//interpolate middle position
			if (zz1 >= 3)
			{
				for (int i = 1; i<zz1-1; ++i)
				{
					for ( int j = 0; j<zz1; ++j)
					{
						float dis1 = i;
						float dis2 = zz1-i;
						float *p1 = (float*)data.ptr(0, j);
						float *p2 = (float*)data.ptr(zz1-1, j);
						float *p = (float *)data.ptr(i, j);
						for (int k = 0; k<3; ++k)
						{
							p[k] = p1[k] * dis2/zz1 + p2[k] * dis1/zz1;
						}
					}
				}
			}
			//set the value
			cv::Rect r1(x, y, sz, sz);
			data(cv::Rect(0, 0, sz, sz)).copyTo(AvectorMap(r1));
		}
	}
	else 
	{
		for (int i = 0; i<4; ++i)
		{
			if (node->_children[i])
			{
				InterpolateAvector(node->_children[i], AvectorMap);
			}
		}
	}
	
	

}

void CvxAdaptiveQuadTree::planeEquation(QtNode *node, vector<map<int, double>> &leftMap, vector<double> &rightVec, bool appliedToEdgeArea)
{
	if (node == NULL)
	{
		node = m_root;
		planeEquation(node, leftMap, rightVec);
	}
	else if (node->isLeaf())
	{
		QtData *pdata = (QtData *)node->_data;
		assert(pdata);
		if (pdata->_edge == false || appliedToEdgeArea)
		{
			double epsilon = 0.01;
			double sqrt_epsilon = sqrt(epsilon);
			double M[9+3][3] = {0};
			vector<unsigned int> idxVec;
			vector<double> shadingValueVec;
			vector<unsigned int> szVec;    //leaf node size

			//gather data from neighbors and self node
			//Gather M matrix
			CvxQuadTree::GatherData(this, node, M, idxVec, shadingValueVec, szVec, false);
			int nTotal = idxVec.size() + 3;
			M[nTotal-3][0] = sqrt_epsilon; M[nTotal-3][1] = 0; M[nTotal-3][2] = 0;
			M[nTotal-2][0] = 0; M[nTotal-2][1] = sqrt_epsilon; M[nTotal-2][2] = 0;
			M[nTotal-1][0] = 0; M[nTotal-1][1] = 0; M[nTotal-1][2] = sqrt_epsilon;

			Mat Mi  = Mat(nTotal,3,CV_64F, M);
			Mat Mit = Mi.t();
			Mat MitMi = Mit * Mi;
			Mat MitMi_inv = MitMi.inv();
			Mat Ni = Mi * MitMi_inv * Mit;
			Ni = Mat::eye(Ni.rows, Ni.cols, CV_64F) - Ni;

			double *pNi = (double*)(Ni.data);
			for (int j = 0; j<Ni.rows; ++j)
			{
				map<int, double> imap;
				double rightVal = 0.0;
				for (int k = 0; k<Ni.cols-3; ++k)
				{
					int niIdx = j * Ni.cols + k;
					float wt = 1.0;
					if (idxVec[k] == -1)  // known shading value
					{
						rightVal -= pNi[niIdx] * shadingValueVec[k] * wt;								
					}
					else
					{						
						int sIdx   = idxVec[k];
						imap[sIdx] = pNi[niIdx] * wt;	
					}
				}
				if (!imap.empty()) //at least one unknown
				{
					leftMap.push_back(imap);
					rightVec.push_back(rightVal);
				}
			}

			//add trend to minimal shading value
			if (pdata->_idx != -1)
			{
				map<int, double> imap;
				double rightVal = 0.0;
				double wt = 0.001;
				imap[pdata->_idx] = 1.0 * wt;
				rightVal = pdata->_minSha * wt;
				leftMap.push_back(imap);
				rightVec.push_back(rightVal);
			}
		}		
	}
	else
	{
		for (int i = 0; i<4; ++i)
		{
			if (node->_children[i] != NULL)
			{
				planeEquation(node->_children[i], leftMap, rightVec);
			}			
		}
	}

}
void CvxAdaptiveQuadTree::edgeEquation(QtNode *node, vector<map<int, double>> &leftMap, 
									   vector<double> &rightVec)
{
	if (node == NULL)
	{
		node = m_root;
		edgeEquation(node, leftMap, rightVec);
	}
	else if (node->isLeaf())
	{
		QtData *pdata = (QtData *)node->_data;
		assert(pdata);

		if (pdata->_edge == false)
		{
			return;
		}

		//fix some shading value in the edge
		{
			int idx = pdata->_idx;
			if (idx != -1)
			{
				double wt = 0.5;
				map<int, double> imap;
				imap[idx] = wt * 1.0;				
				double v_right = wt * pdata->_minSha;
				leftMap.push_back(imap);
				rightVec.push_back(v_right);				
			}			
		}
	}
	else
	{
		for (int i = 0; i<4; ++i)
		{
			if (node->_children[i])
			{
				edgeEquation(node->_children[i], leftMap, rightVec);
			}			
		}
	}
}

void CvxAdaptiveQuadTree::matchNodeEquation(QtNode *node, vector<map<int, double>> &leftMap, vector<double> &rightVec)
{
	if(node == NULL)
	{
		matchNodeEquation(m_root, leftMap, rightVec);
	}
	else if(node->isLeaf())
	{
		QtData *pdata = (QtData *)node->_data;
		assert(pdata);
		if(pdata->_bestMatchNode)
		{
			QtData *d1 = pdata;
			QtData *d2 = (QtData *)(pdata->_bestMatchNode->_data);
			assert(d2);

			int idx1 = d1->_idx;
			int idx2 = d1->_idx;
			if(idx1 != -1 && idx2 != -1)
			{
				int rgb1[3] = {d1->_r, d1->_g, d1->_b};
				int rgb2[3] = {d2->_r, d2->_g, d2->_b};
				
				for(int i = 0; i<3; i++)
				{
					//coherent reflectance: s_i * I_j - s_j * I_i = 0
					map<int, double> imap;
					double right = 0.0;

					imap[idx1] = rgb2[i];
					imap[idx2] = -rgb1[i];

					leftMap.push_back(imap);
					rightVec.push_back(right);
				}
			}
		}
	}
	else
	{
		for(int i = 0; i<4; i++)
		{
			if(node->_children[i])
			{
				matchNodeEquation(node->_children[i], leftMap, rightVec);
			}
		}		
	}
}



void CvxAdaptiveQuadTree::AvectorEquation(QtNode *node, vector<map<int, double>> &leftMap, vector<double> &rightVec)
{
	if (node == NULL)
	{
		AvectorEquation(m_root, leftMap, rightVec);
	}
	else if (node->isLeaf() && node->_data)
	{
		QtData *pdata = (QtData *)node->_data;
		assert(pdata);

		double epsilon = 0.01;
		double sqrt_epsilon = sqrt(epsilon);
		double M[9][3] = {0};
		vector<unsigned int> idxVec;
		vector<double> shadingValueVec;
		vector<unsigned int> szVec;    //leaf node size

		//gather data from neighbors and self node
		//Gather M matrix
		GatherData(this, node, M, idxVec, shadingValueVec, szVec, true);
		int idx = pdata->_nodeIdx;
		//M is r g b
		for (int j = 0; j<idxVec.size(); ++j)
		{
			//equation 8
			{
				map<int, double> imap;
				double rightVal = 0.0;

				imap[3*idx] = M[j][0];
				imap[3*idx + 1] = M[j][1];
				imap[3*idx + 2] = M[j][2];

				rightVal = shadingValueVec[j];

				leftMap.push_back(imap);
				rightVec.push_back(rightVal);
			}
			//add smooth ?
			int otherIdx = idxVec[j];
			if (pdata->_edge == false)
			{
				if (idx != otherIdx)
				{
					for (int k = 0; k<3; ++k)
					{
						map<int, double> imap;
						double rightVal = 0.0;
						
						imap[3*idx+k] = 1.0;
						imap[3*otherIdx+k] = 1.0;			

 						leftMap.push_back(imap);
 						rightVec.push_back(rightVal);
					}
					
				}
			}			
		}
		{
			map<int, double> imap;
			double rightVal = 0.0;
			imap[3*idx] = sqrt_epsilon;
			leftMap.push_back(imap);
			rightVec.push_back(rightVal);
		}

		{
			map<int, double> imap;
			double rightVal = 0.0;
			imap[3*idx+1] = sqrt_epsilon;
			leftMap.push_back(imap);
			rightVec.push_back(rightVal);
		}

		{
			map<int, double> imap;
			double rightVal = 0.0;
			imap[3*idx+2] = sqrt_epsilon;
			leftMap.push_back(imap);
			rightVec.push_back(rightVal);
		}

	}
	else
	{
		for (int i = 0; i<4; ++i)
		{
			if (node->_children[i])
			{
				AvectorEquation(node->_children[i], leftMap, rightVec);
			}			
		}
	}
}

void CvxAdaptiveQuadTree::setAvector(Mat &AvectorMap, QtNode *node, const vector<double> &rst)
{
	if (node == NULL)
	{
		setAvector(AvectorMap, m_root, rst);
	}
	else if (node->isLeaf())
	{
		if (node->_data != NULL)
		{
			int sz = 1<<node->_level;
			QtData *pData = (QtData *)(node->_data);
			assert(pData);
			int idx = pData->_nodeIdx;
			pData->_avector[0] = rst[3*idx + 0];
			pData->_avector[1] = rst[3*idx + 1];
			pData->_avector[2] = rst[3*idx + 2];						
			double av[3];
			av[0] = rst[3*idx];
			av[1] = rst[3*idx+1];
			av[2] = rst[3*idx+2];
			double ss = pData->_r * av[0] + pData->_g *av[1] + pData->_b * av[2];
			if (sz != 1)
			{
				cv::Rect r1 = Rect(node->_xLocCode, node->_yLocCode, sz, sz);
				Mat m1 = AvectorMap(r1);
				m1 = Scalar(av[2], av[1], av[0], 1.0);
			}
			else
			{
				//b g r
				float *p = (float *)AvectorMap.ptr(node->_yLocCode, node->_xLocCode);
				p[0] = av[2];
				p[1] = av[1];
				p[2] = av[0];
			}				
		}
	}
	else
	{
		for (int i =0 ; i<4; ++i)
		{
			if (node->_children[i] != NULL)
			{
				setAvector(AvectorMap, node->_children[i], rst);
			}			
		}
	}
}

void CvxAdaptiveQuadTree::setAvector(QtNode *node, const vector<double> &rst)
{
	if (node == NULL)
	{
		setAvector(m_root, rst);
	}
	else if (node->isLeaf())
	{
		if (node->_data != NULL)
		{
			int sz = 1<<node->_level;			
			QtData *pData = (QtData *)(node->_data);
			assert(pData);
			int idx = pData->_nodeIdx;
			pData->_avector[0] = (float)rst[3*idx + 0];
			pData->_avector[1] = (float)rst[3*idx + 1];
			pData->_avector[2] = (float)rst[3*idx + 2];						
		}
	}
	else
	{
		for (int i =0 ; i<4; ++i)
		{
			if (node->_children[i] != NULL)
			{
				setAvector(node->_children[i], rst);
			}			
		}
	}

}
